#include "ComponentPathfinding.h"
#include "ComponentPilot.h"
#include "Components/ComponentCreature.h"
#include "Components/ComponentLocomotion.h"
#include "Components/ComponentBody.h"
#include "GameSingleton.h"
#include "Managers/GameManager.h"
#include "Subsystems/SubsystemTimeOfDay.h"
#include "SubsystemPathfinding.h"
#include "GameRandom.hpp"
#include "GameRegistry.hpp"
#include "GameSingleton.h"
#include "GameThreadPool.hpp"

#include "Log.h"


namespace PocketSurvival
{
    // ************** 【状态机】 **************

    ComponentPathfinding::StateMachine::StateMachine(ComponentPathfinding *m_pathfinding) : m_pathfinding(m_pathfinding), EasyMachine(MachineState::Stopped) 
    {
        enter();
    }

    void ComponentPathfinding::StateMachine::enter()
    {
        switch (m_currentState)
        {
        case MachineState::Stopped:
            MACHINE_DELEGATE()
            {
                // Log::Info("Stopped");
                m_pathfinding->stop();
                m_pathfinding->randomMoveCount = 0;
            }
            break;
        case MachineState::MovingDirect:
            MACHINE_DELEGATE()
            {
                // Log::Info("MovingDirect");
                m_pathfinding->isStuck = false;
                m_pathfinding->destinationChanged = true;
            }
            break;
        case MachineState::SearchingForPath:
            MACHINE_DELEGATE()
            {
                // Log::Info("SearchingForPath");
                m_pathfinding->pathfindingResult.IsCompleted = false;
                m_pathfinding->pathfindingResult.IsInProgress = false;
            }
            break;
        case MachineState::MovingWithPath:
            MACHINE_DELEGATE()
            {
                // Log::Info("MovingWithPath");
                m_pathfinding->componentPilot->stop();
                m_pathfinding->randomMoveCount = 0;
            }
            break;
        case MachineState::MovingRandomly:
            MACHINE_DELEGATE()
            {
                // Log::Info("MovingRandomly");
                Vector3 position;
                m_pathfinding->componentCreature->componentBody->getPosition(position);

                Vector3 pos(pos + Vector3(5.0f * GameSingleton::gameRandom->getFloat(-1.0f, 1.0f), 0.0f, 5.0f * GameSingleton::gameRandom->getFloat(-1.0f, 1.0f)));
                m_pathfinding->componentPilot->setDestination(pos, 1.0f, 1.0f, true, false, false, m_pathfinding->doNotAvoidBody);
                m_pathfinding->randomMoveCount++;
            }
            break;
        case MachineState::Stuck:
            MACHINE_DELEGATE()
            {
                // Log::Info("Stuck");
                m_pathfinding->isStuck = true;
            }
            break;
        }
    }

    void ComponentPathfinding::StateMachine::update(float dt)
    {
        switch (m_currentState)
        {
        case MachineState::Stopped:
            MACHINE_DELEGATE()
            {
                if (m_pathfinding->destination != FastVector3::Zero)
                {
                    changeTo(MachineState::MovingDirect);
                }
            }
            break;
        case MachineState::MovingDirect:
            MACHINE_DELEGATE()
            {
                if (m_pathfinding->destination == FastVector3::Zero)
                {
                    changeTo(MachineState::Stopped);
                }
                else if (m_pathfinding->destinationChanged)
                {
                    m_pathfinding->componentPilot->setDestination(
                        m_pathfinding->destination,
                        m_pathfinding->speed,
                        m_pathfinding->range,
                        m_pathfinding->ignoreHeightDifference,
                        m_pathfinding->raycastDestination,
                        m_pathfinding->speed >= 1.0f,
                        m_pathfinding->doNotAvoidBody);

                    m_pathfinding->destinationChanged = false;
                }
                else if (m_pathfinding->componentPilot->destination == FastVector3::Zero)
                {
                    changeTo(MachineState::Stopped);
                }
                else if (m_pathfinding->componentPilot->isStuck)
                {
                    if (m_pathfinding->maxPathfindingPositions > 0 && m_pathfinding->componentCreature->componentLocomotion->walkSpeed > 0.0f)
                    {
                        changeTo(MachineState::SearchingForPath);
                    }
                    else if (m_pathfinding->useRandomMovements)
                    {
                        changeTo(MachineState::MovingRandomly);
                    }
                    else
                    {
                        changeTo(MachineState::Stuck);
                    }
                }
            }
            break;
        case MachineState::SearchingForPath:
            MACHINE_DELEGATE()
            {
                SubsystemTimeOfDay *timeOfDay = GameSingleton::singletonPtr->getSingleton<SubsystemTimeOfDay>();
                SubsystemPathfinding *subsystemPathfinding = GameSingleton::singletonPtr->getSingleton<SubsystemPathfinding>();

                if (m_pathfinding->destination == FastVector3::Zero)
                {
                    changeTo(MachineState::Stopped);
                }
                else if (m_pathfinding->pathfindingResult.IsInProgress == false && (m_pathfinding->lastPathfindingTime > 0 || timeOfDay->getGameTime() - m_pathfinding->lastPathfindingTime > 8.0) && m_pathfinding->pathfindingCongestion < 500.0f)
                {
                    m_pathfinding->lastPathfindingDestination = m_pathfinding->destination;
                    m_pathfinding->lastPathfindingTime = timeOfDay->getGameTime();

                    Vector3 position;
                    m_pathfinding->componentCreature->componentBody->getPosition(position);

                    subsystemPathfinding->pathSearch(
                        position + Vector3(0.0f, 0.01f, 0.0f),
                        m_pathfinding->destination + Vector3(0.0f, 0.01f, 0.0f),
                        1.0f,
                        m_pathfinding->componentCreature->componentBody->boxSize,
                        m_pathfinding->maxPathfindingPositions,
                        &(m_pathfinding->pathfindingResult));
                }
                else if (m_pathfinding->useRandomMovements)
                {
                    changeTo(MachineState::MovingRandomly);
                }
                if (m_pathfinding->pathfindingResult.IsCompleted)
                {
                    m_pathfinding->pathfindingCongestion = MathUtils::Min<float>(m_pathfinding->pathfindingCongestion + m_pathfinding->pathfindingResult.PositionChecked, 1000.0f);
                    if (m_pathfinding->pathfindingResult.Path.size() > 0)
                    {
                        changeTo(MachineState::MovingWithPath);
                    }
                    else if (m_pathfinding->useRandomMovements)
                    {
                        changeTo(MachineState::MovingRandomly);
                    }
                    else
                    {
                        changeTo(MachineState::Stuck);
                    }
                }
            }
            break;
        case MachineState::MovingWithPath:
            MACHINE_DELEGATE()
            {
                if (m_pathfinding->destination == FastVector3::Zero)
                {
                    changeTo(MachineState::Stopped);
                }
                else if (m_pathfinding->componentPilot->destination == FastVector3::Zero)
                {
                    if (m_pathfinding->pathfindingResult.Path.size() > 0)
                    {
                        Vector3 value(*(m_pathfinding->pathfindingResult.Path.rbegin()));
                        m_pathfinding->pathfindingResult.Path.erase(m_pathfinding->pathfindingResult.Path.rbegin().base());
                        m_pathfinding->componentPilot->setDestination(value, MathUtils::Min(m_pathfinding->speed, 0.75f), 0.75f, false, false, m_pathfinding->speed >= 1.0f, m_pathfinding->doNotAvoidBody);
                    }
                    else
                    {
                        changeTo(MachineState::MovingDirect);
                    }
                }
                else if (m_pathfinding->componentPilot->isStuck)
                {
                    if (m_pathfinding->useRandomMovements)
                    {
                        changeTo(MachineState::MovingRandomly);
                    }
                    else
                    {
                        changeTo(MachineState::Stuck);
                    }
                }
                else
                {
                    Vector3 position;
                    m_pathfinding->componentCreature->componentBody->getPosition(position);

                    float num = Vector3::DistanceSquared(position, m_pathfinding->destination);
                    if (Vector3::DistanceSquared(m_pathfinding->lastPathfindingDestination, m_pathfinding->destination) > num)
                    {
                        changeTo(MachineState::MovingDirect);
                    }
                }
            }
            break;
        case MachineState::MovingRandomly:
            MACHINE_DELEGATE()
            {
                if (m_pathfinding->destination == FastVector3::Zero)
                {
                    changeTo(MachineState::Stopped);
                }
                else if (m_pathfinding->randomMoveCount > 3)
                {
                    changeTo(MachineState::Stuck);
                }
                else if (m_pathfinding->componentPilot->isStuck || m_pathfinding->componentPilot->destination == FastVector3::Zero)
                {
                    changeTo(MachineState::MovingDirect);
                }
            }
            break;
        case MachineState::Stuck:
            MACHINE_DELEGATE()
            {
                if (m_pathfinding->destination == FastVector3::Zero)
                {
                    changeTo(MachineState::Stopped);
                }
                else if (m_pathfinding->destinationChanged)
                {
                    m_pathfinding->destinationChanged = false;
                    changeTo(MachineState::MovingDirect);
                }
            }
            break;
        }
    }

    void ComponentPathfinding::StateMachine::leave() {}

    // **************************************

    ComponentPathfinding::~ComponentPathfinding()
    {
        delete m_stateMachine;
    }

    void ComponentPathfinding::update(float dt)
    {
        SubsystemTimeOfDay *timeOfDay = GameSingleton::singletonPtr->getSingleton<SubsystemTimeOfDay>();
        if (timeOfDay->getGameTime() >= nextUpdateTime)
        {
            float num = GameSingleton::gameRandom->getFloat(0.08f, 0.12f);
            nextUpdateTime = timeOfDay->getGameTime() + num;
            pathfindingCongestion = MathUtils::Max<float>(pathfindingCongestion - 20.0f * num, 0.0f);
            m_stateMachine->update(dt);
        }
    }

    void ComponentPathfinding::setDestination(
        const Vector3 &theDestination,
        float theSpeed,
        float theRange,
        int32_t theMaxPathfindingPositions,
        bool theUseRandomMovements,
        bool theIgnoreHeightDifference,
        bool theRaycastDestination,
        ComponentBody *theDoNotAvoidBody)
    {
        destination = theDestination;
        // if (theDestination != nullptr)
        // {
        //     Log::Info(fmt::format("{} => {}", componentCreature->componentBody->position.toString(), theDestination->toString()));
        // }

        // Log::Info(fmt::format("{} => {}", componentCreature->componentBody->position.toString(), theDestination.toString()));

        speed = theSpeed;
        range = theRange;
        maxPathfindingPositions = theMaxPathfindingPositions;
        useRandomMovements = theUseRandomMovements;
        ignoreHeightDifference = theIgnoreHeightDifference;
        raycastDestination = theRaycastDestination;
        doNotAvoidBody = theDoNotAvoidBody;
        destinationChanged = true;
        nextUpdateTime = 0.0;
    }

    void ComponentPathfinding::stop()
    {
        setDestination(FastVector3::Zero, 0.0f, 0.0f, 0, false, false, false, nullptr);
        componentPilot->stop();
        isStuck = false;
    }

    static const std::string componentName = "Pathfinding";
    const std::string &ComponentPathfinding::getName() const
    {
        return componentName;
    }
    const std::string &ComponentPathfinding::GetName()
    {
        return componentName;
    }
    void ComponentPathfinding::load(const nlohmann::json &jsonData)
    {
        componentCreature = GameSingleton::gameRegistry->get<ComponentCreature *>(entity);
        componentPilot = GameSingleton::gameRegistry->get<ComponentPilot *>(entity);

        m_stateMachine = new StateMachine(this);
    }

}
